#include <AccelStepper.h>

// ================== PIN DEFINITIONS ==================
#define STEP_PIN D1      // STEP signal to driver
#define DIR_PIN  D0      // DIR signal to driver
#define EN_PIN   D3      // ENABLE (LOW = enabled)

#define M0_PIN D2        // Microstepping pins
#define M1_PIN D4
#define M2_PIN D5

#define HALL_PIN D8      // Hall sensor (analog input)

// ================== HALL THRESHOLDS ==================
// Tune these based on your sensor readings
#define ON_THRESHOLD  3800   // Enter magnet zone above this value
#define OFF_THRESHOLD 3400   // Exit magnet zone below this value

// ================== SPEED SETTINGS ==================
// Steps per second (sign defines direction)
#define SEARCH_SPEED    800   // Fast CW search
#define BACKOFF_SPEED   400   // CCW backoff
#define APPROACH_SPEED  200   // Slow CW final approach (high accuracy)

// ================== STEPPER OBJECT ==================
AccelStepper stepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);

// ================== HALL FILTER STATE ==================
float filtered = 0.0f;  // Low-pass filtered sensor value
bool  hallState = false; // Debounced digital state (true = magnet present)

// ================== HOMING STATE MACHINE ==================
enum HomingState {
  SEARCH,    // Rotate CW until magnet is first detected (edge)
  BACKOFF,   // Rotate CCW until magnet is fully released
  APPROACH,  // Rotate CW slowly to detect edge again (precise)
  DONE       // Homing complete
};

HomingState state = SEARCH;

// ================== SETUP ==================
void setup() {
  Serial.begin(115200);

  // ---- Enable driver ----
  pinMode(EN_PIN, OUTPUT);
  digitalWrite(EN_PIN, LOW);   // LOW = driver enabled

  // ---- Set microstepping (1/16 for DRV8825) ----
  pinMode(M0_PIN, OUTPUT);
  pinMode(M1_PIN, OUTPUT);
  pinMode(M2_PIN, OUTPUT);

  digitalWrite(M0_PIN, HIGH);
  digitalWrite(M1_PIN, HIGH);
  digitalWrite(M2_PIN, LOW);

  // ---- Initialize Hall filter baseline ----
  filtered = analogRead(HALL_PIN);

  // ---- Configure stepper limits ----
  stepper.setMaxSpeed(1200);      // Upper limit (must exceed SEARCH_SPEED)
  stepper.setAcceleration(800);   // Not used by runSpeed(), but kept for later use

  Serial.println("Homing start...");
}

// ================== HALL SENSOR (FILTER + HYSTERESIS) ==================
bool readHallDigital() {
  int raw = analogRead(HALL_PIN);     // Read analog value

  // ---- Low-pass filter (smooth noise) ----
  filtered = 0.8f * filtered + 0.2f * raw;

  // ---- Hysteresis (prevents flicker near threshold) ----
  if (!hallState && filtered > ON_THRESHOLD)  hallState = true;   // enter zone
  if (hallState && filtered < OFF_THRESHOLD)  hallState = false;  // exit zone

  return hallState;
}

// ================== LOOP ==================
void loop() {

  // Track previous Hall state to detect edges (transitions)
  static bool lastHall = false;
  bool currentHall = readHallDigital();

  switch (state) {

    // -------- STAGE 1: FAST SEARCH (CW) --------
    case SEARCH:
      // Positive speed = CW
      stepper.setSpeed(SEARCH_SPEED);
      stepper.runSpeed();   // Non-blocking continuous stepping

      // Detect rising edge: entering magnet zone
      if (currentHall && !lastHall) {
        Serial.println("SEARCH: magnet detected (edge)");
        state = BACKOFF;    // Move to backoff stage
      }
      break;

    // -------- STAGE 2: BACKOFF (CCW) --------
    case BACKOFF:
      // Negative speed = CCW
      stepper.setSpeed(-BACKOFF_SPEED);
      stepper.runSpeed();

      // Detect falling edge: fully exited magnet zone
      if (!currentHall && lastHall) {
        Serial.println("BACKOFF: exited magnet zone");
        state = APPROACH;   // Proceed to precise approach
      }
      break;

    // -------- STAGE 3: SLOW APPROACH (CW) --------
    case APPROACH:
      // Slow CW motion for precise edge detection
      stepper.setSpeed(APPROACH_SPEED);
      stepper.runSpeed();

      // Detect rising edge again (high repeatability)
      if (currentHall && !lastHall) {
        Serial.println("APPROACH: magnet detected (precise edge)");

        // Stop motion
        stepper.setSpeed(0);

        // Define HOME position (c0) as 0 steps
        stepper.setCurrentPosition(0);

        Serial.println("HOMING COMPLETE → c0 = 0 steps");

        state = DONE;
      }
      break;

    // -------- STAGE 4: DONE --------
    case DONE:
      // Motor is stopped; ready for positioning logic
      // (No further action here)
      break;
  }

  // Update last state for edge detection in next loop
  lastHall = currentHall;
}